#include "robot_gait.h"

float faai = 0.5f;		// 占空比
float Ts = 0.7f;		// 周期

gait_walk_t gait_walk[2];

union _point_t set_point;

uint8_t gait_walk_EN = 0;

/**
 * @brief  机器人步态初始化
 * @param  
 * @retval 无
 */
void robot_gait_init(void)
{
	
}

/**
 * @brief  机器人行走步态
 * @param  
 * @retval 无
 */
void robot_gait_walk(gait_walk_t *wa, float x, float y, float base_z, float z)
{
	float faai_2 = faai / 2.0f;
	
	float sx,sy,sz;
	
	uint8_t gait_state;
	
	// 判定当前时间点应落于步态哪个点
	if (wa->time < Ts * faai_2) {
		gait_state = 0;
	} else if (wa->time < Ts * faai_2 + Ts * faai) {
		gait_state = 1;
	} else if (Ts) {
		gait_state = 2;
	} else {
		gait_state = 3;
	}
	
	
	#define get_dis_rate(time, start_time, end_time)				((float)(time-start_time) / (float)(end_time-start_time))
	#define get_2pi(time, start_time, end_time)							(get_dis_rate(time, start_time, end_time) * 2.0f * PI)
	
	
	// 步态状态
	switch(gait_state) {
		// 前半段往后走		0-0.25
		case 0: {
			float rate = get_dis_rate(wa->time, 0.f, Ts * faai_2);
			
			sx = -x * rate * 0.5f;
			sy = -y * rate * 0.5f;
			sz = base_z;
			break;
		}
		
		// 中间段往后走		0.25-0.75
		case 1: {
			float rate = get_dis_rate(wa->time, Ts * faai_2, Ts * (faai+faai_2));
			float sigma = get_2pi(wa->time, Ts * faai_2, Ts * (faai+faai_2)) - PI;
			float z_err = z - base_z;
				
			sx = (-x * 0.5f) + (x * rate);
			sy = (-y * 0.5f) + (y * rate);
			sz = base_z + (z_err * 0.5f * (1 + cosf(sigma)));
			break;
		}
		
		// 后半段往后走		0.75-1.00
		case 2: {
			float rate = get_dis_rate(wa->time, Ts * (faai+faai_2), 1.0f);
			
			sx = (x * 0.5f) - (x * rate * 0.5f);
			sy = (y * 0.5f) - (y * rate * 0.5f);
			sz = base_z;
			break;
		}
		
			// 中间段往后走		1.00-~
		case 3: {
			
			break;
		}
	}
	
	wa->point.x = sx;
	wa->point.y = sy;
	wa->point.z = sz;
	
	// 时间往前走
	wa->time += robot_ctrl_cycle * wa->en;
	
	// 时间重新开始，步态具有周期性
	if (wa->time > Ts) {
		wa->time = 0;
	}
	
	if (wa->en == 0) {
		wa->time = 0;
	}
}
uint8_t state = 0;
/**
 * @brief  机器人步态规划
 * @param  
 * @retval 无
 */
void robot_gait_plann(void)
{
	float set_x = 0.f;
	float set_y = 0.f;
	
	switch(state) {
		case 0:{	//等待
			
			if (fabs(set_point.x) > 10 || fabs(set_point.y) > 10) {
				state = 1;
			}
			
			break;
		}
		case 1:{	//启动
			set_x = 0.f;
			set_y = 0.f;
		
			// 先使能一条腿
			gait_walk[0].en = 1;
			
			// 第一条腿运动一半启动第二条腿
			if (gait_walk[0].time > Ts*faai) {
				gait_walk[1].en = 1;
				state = 2;
			}
			break;
		}
		
		case 2:{	//运动
			set_x = set_point.x;
			set_y = set_point.y;
			
			if (fabs(set_point.x) < 10 && fabs(set_point.y) < 10 ) {
				gait_walk_EN = 1;
			}
			
			if (gait_walk_EN && gait_walk[0].time < robot_ctrl_cycle) {
				state = 3;
				gait_walk_EN = 0;
				gait_walk[0].en = 0;
			}
			break;
		}
		
		case 3:{	//结束
			set_x = 0.f;
			set_y = 0.f;
			if (gait_walk[1].time < robot_ctrl_cycle) {
				gait_walk[1].en = 0;
				state = 0;
			}
		}
	}
	
	for (uint8_t i=0; i < 2; i++) {
		robot_gait_walk(&gait_walk[i], set_x, set_y, -70, -30);
	}
}

void gait_joint_to_point(union _point_t * p, union _point_t *s_p, int8_t y_flag)
{
	p->x = s_p->x;
	p->y = s_p->y * y_flag;
	p->z = s_p->z;
}

/**
 * @brief  步态脚分配
 * @param  
 * @retval 无
 */
void robot_gait_joint_deal(void)
{
	gait_joint_to_point(&footEnd_point[0], &gait_walk[0].point, 1);
	gait_joint_to_point(&footEnd_point[1], &gait_walk[1].point, 1);
	gait_joint_to_point(&footEnd_point[2], &gait_walk[0].point, 1);

	gait_joint_to_point(&footEnd_point[3], &gait_walk[1].point, -1);
	gait_joint_to_point(&footEnd_point[4], &gait_walk[0].point, -1);
	gait_joint_to_point(&footEnd_point[5], &gait_walk[1].point, -1);

	// for (uint8_t i=0; i < 3; i++) {
	// 	footEnd_point[i*2].x = gait_walk[0].point.x;
	// 	footEnd_point[i*2].y = gait_walk[0].point.y;
	// 	footEnd_point[i*2].z = gait_walk[0].point.z;
		
	// 	footEnd_point[i*2+1].x = gait_walk[1].point.x;
	// 	footEnd_point[i*2+1].y = gait_walk[1].point.y;
	// 	footEnd_point[i*2+1].z = gait_walk[1].point.z;
	// }
	
}

